import numpy as np
import logging
from collections import defaultdict
from queue import Queue, PriorityQueue
import random
from utils import solve_position
import time

random.seed(time.time())


class IterativeMutisidedPosition:
    """
    迭代多边定位算法：
        算法思想:
            利用已知锚节点 (Anchor)和未知节点的几组对应关系 (两点间距离)，利用三边定位，对未知节点进行定位。
            (某个未知节点只要已知3个及以上锚节点与其距离即可对其进行定位。)
        定位成功后的未知节点转换为锚节点，可辅助对其他未知节点定位。
        不断迭代定位过程，直到锚节点集合元素个数在前后两次迭代中数量不变，则迭代终止。
         (x,y)为未知节点， (x_1, y_1)...等为锚节点, 未知节点和锚节点间距离为dist。
        (x-x_1)^2+(y-y_1)^2=dist_1^2
        (x-x_2)^2+(y-y_2)^2=dist_2^2
        (x-x_3)^2+(y-y_3)^2=dist_3^2
        通过最小二乘法或其他方法求解 (x,y)，然后将其加入锚节点列表。
    """

    def __init__(self, anchor_points_dict, graph):
        # idx, x, y
        self.anchor_points: dict = anchor_points_dict
        self.graph: dict = graph
        logging.debug("初始化 IterativeMutisidedPosition 完成")
        logging.debug(f"anchor_points: {self.anchor_points}")

    def run(self):
        logging.debug("开始迭代多边定位")
        # 计算出的节点 {idx}
        cal_points = set()
        # 已知的锚节点 {idx: (x, y)}
        archor_points = self.anchor_points
        graph = self.graph

        def is_visited(x):
            return x in archor_points

        # 记录被锚节点扩展的次数
        expended = defaultdict(set)
        """
        很有意思, FIFO 
        比   set    随机    得到的结果更好
        也比 PriorityQueue  得到的结果更好
        """
        # posible_archor = set()
        posible_archor = Queue()
        # posible_archor = PriorityQueue()
        archor_points_name = list(archor_points.keys())
        random.shuffle(archor_points_name)
        for u in archor_points_name:
            neis = graph.get_neighbors(u)
            for v in neis:
                if is_visited(v):
                    continue
                expended[v].add(u)
                if len(expended[v]) >= 3:
                    # posible_archor.add(v)
                    posible_archor.put(v)
                    # posible_archor.put((-len(expended[v]), v))

        # while len(posible_archor):
        while not posible_archor.empty():
            # 需要被更新的节点
            # u = posible_archor.pop()
            u = posible_archor.get()
            # _,u = posible_archor.get()
            if is_visited(u):
                continue
            archors = expended[u]
            logging.debug(f"定位节点{u}: ")
            logging.debug(f"\t锚节点{archors}")
            ux, uy = solve_position(
                [graph.get_distance(u, v) for v in archors],
                [archor_points[v] for v in archors],
            )
            logging.debug(f"\t结果为({ux},{uy})")

            # 标记已计算过的节点
            archor_points[u] = np.array([ux, uy])
            cal_points.add(u)

            neis = graph.get_neighbors(u)
            for v in neis:
                if is_visited(v):
                    continue
                expended[v].add(u)
                if len(expended[v]) >= 3:
                    # posible_archor.add(v)
                    posible_archor.put(v)
                    # posible_archor.put((-len(expended[v]), v))

        return {idx: archor_points[idx] for idx in cal_points}
